/**
 *=============================================================================
 *  @copyright (C)      2025CCUT_1024lab_Damocles
 *
 *  @file    template.c
 *  @brief   封装模块代码规范
 *
 *  @note    使用方法
 *
 *  @attention 注意事项
 *-----------------------------------------------------------------------------
 *  @version    @author    @date    @note
 *  V1.0.0      Cherno    250808    done
 *
 *==============================================================================
 */

/*==============================Includes======================================*/
#include "dvc_djimotor.h"
/*==============================PrivateMacros=================================*/
#define get_motor_measure(ptr, data)                             \
	{                                                            \
		(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);     \
		(ptr)->rpm = (uint16_t)((data)[2] << 8 | (data)[3]);     \
		(ptr)->current = (uint16_t)((data)[4] << 8 | (data)[5]); \
		(ptr)->temperature = (data)[6];                          \
	}
/*==============================PrivateTypes==================================*/

/*==============================PrivateVariables==============================*/
GM6020 GM6020_1;

/*==============================PrivateFunctionDeclarations===================*/

/*==============================FunctionPrototypes============================*/

/**
 * @brief 分配CAN发送缓冲区
 *
 * @param hcan CAN编号
 * @param id CAN ID
 * @return uint8_t* 缓冲区指针
 */
uint8_t *allocate_tx_data(CAN_HandleTypeDef *hcan, dji_id_e id)
{
	uint8_t *tmp_tx_data_ptr=NULL;
	if (hcan==&hcan1) {
		switch (id) {
			case (DJI_ID_0x201): {
				tmp_tx_data_ptr = &(can1_0x200_tx_data[0]);
			} break;
			case (DJI_ID_0x202): {
				tmp_tx_data_ptr = &(can1_0x200_tx_data[2]);
			} break;
			case (DJI_ID_0x203): {
				tmp_tx_data_ptr = &(can1_0x200_tx_data[4]);
			} break;
			case (DJI_ID_0x204): {
				tmp_tx_data_ptr = &(can1_0x200_tx_data[6]);
			} break;
			case (DJI_ID_0x205): {
				tmp_tx_data_ptr = &(can1_0x1ff_tx_data[0]);
			} break;
			case (DJI_ID_0x206): {
				tmp_tx_data_ptr = &(can1_0x1ff_tx_data[2]);
			} break;
			case (DJI_ID_0x207): {
				tmp_tx_data_ptr = &(can1_0x1ff_tx_data[4]);
			} break;
			case (DJI_ID_0x208): {
				tmp_tx_data_ptr = &(can1_0x1ff_tx_data[6]);
			} break;
			case (DJI_ID_0x209): {
				tmp_tx_data_ptr = &(can1_0x2ff_tx_data[0]);
			} break;
			case (DJI_ID_0x20A): {
				tmp_tx_data_ptr = &(can1_0x2ff_tx_data[2]);
			} break;
			case (DJI_ID_0x20B): {
				tmp_tx_data_ptr = &(can1_0x2ff_tx_data[4]);
			} break;
			default: {
				tmp_tx_data_ptr = NULL;
			}break;
		}
	} else if (hcan==&hcan2) {
		switch (id) {
			case (DJI_ID_0x201): {
				tmp_tx_data_ptr = &(can2_0x200_tx_data[0]);
			} break;
			case (DJI_ID_0x202): {
				tmp_tx_data_ptr = &(can2_0x200_tx_data[2]);
			} break;
			case (DJI_ID_0x203): {
				tmp_tx_data_ptr = &(can2_0x200_tx_data[4]);
			} break;
			case (DJI_ID_0x204): {
				tmp_tx_data_ptr = &(can2_0x200_tx_data[6]);
			} break;
			case (DJI_ID_0x205): {
				tmp_tx_data_ptr = &(can2_0x1ff_tx_data[0]);
			} break;
			case (DJI_ID_0x206): {
				tmp_tx_data_ptr = &(can2_0x1ff_tx_data[2]);
			} break;
			case (DJI_ID_0x207): {
				tmp_tx_data_ptr = &(can2_0x1ff_tx_data[4]);
			} break;
			case (DJI_ID_0x208): {
				tmp_tx_data_ptr = &(can2_0x1ff_tx_data[6]);
			} break;
			case (DJI_ID_0x209): {
				tmp_tx_data_ptr = &(can2_0x2ff_tx_data[0]);
			} break;
			case (DJI_ID_0x20A): {
				tmp_tx_data_ptr = &(can2_0x2ff_tx_data[2]);
			} break;
			case (DJI_ID_0x20B): {
				tmp_tx_data_ptr = &(can2_0x2ff_tx_data[4]);
			} break;
			default: {
				tmp_tx_data_ptr = NULL;
			}
		}
	}
	return (tmp_tx_data_ptr);
}

void GM6020::init(CAN_HandleTypeDef *_hcan, dji_id_e _id, dji_control_e _method,
                  int32_t _encoder_offset)
{
	if (_hcan->Instance == CAN1) {
		can_manager = &can1_manager;
	} else if (_hcan->Instance == CAN2) {
		can_manager = &can2_manager;
	}
	id = _id;
	method = _method;
	encoder_offset = _encoder_offset;
	can_tx_data = allocate_tx_data(_hcan, _id);
}


void GM6020::data_pross()
{
	static motor_measure_t tmp_measure;
	int16_t delta_encoder;
	// 数据处理过程
	get_motor_measure(&tmp_measure, can_manager->rx_buffer.data);

	// 计算圈数与总编码器值
	delta_encoder = tmp_measure.ecd - data.pre_encoder;
	if (delta_encoder < -encoder_num_pre_round / 2) {
		// 正方向转过了一圈
		data.total_round++;
	} else if (delta_encoder > encoder_num_pre_round / 2) {
		// 反方向转过了一圈
		data.total_round--;
	}	
	data.total_encoder = data.total_round * encoder_num_pre_round +
	                     tmp_measure.ecd + encoder_offset;

	data.angle=(float)data.total_encoder/(float)encoder_num_pre_round*2.0f*PI;
	data.omega = (float)tmp_measure.rpm*RPM_TO_OMEGA;
	data.torque = tmp_measure.current;
	data.temperature = tmp_measure.temperature+CELSIUS_TO_KELVIN;

	// 存储预备信息
	data.pre_encoder=tmp_measure.ecd;
}

void GM6020::CAN_RxCpltCallback(uint8_t *rx_data)
{
	data_pross();
}

void GM6020::output()
{
	can_tx_data[0] = (int16_t)out >> 8;
	can_tx_data[1] = (int16_t)out;
}

void GM6020::pid_calc()
{
	switch (method) {
		case (OPENLOOP_CTRL): {
			out = target_torque;
		} break;
		case (TORQUE_CTRL): {
			pid_torque.set_target(target_torque);
			pid_torque.set_ref(data.torque);
			pid_torque.calc();
			out = pid_torque.get_out();
		} break;
		case (OMEGA_CTRL): {
			pid_omega.set_target(target_omega);
			pid_omega.set_ref(data.omega);
			pid_omega.calc();
			out = pid_omega.get_out();
		} break;
		case (ANGLE_CTRL): {
			pid_angle.set_target(target_angle);
			pid_angle.set_ref(data.angle);
			pid_angle.calc();
			target_omega = pid_angle.get_out();

			pid_omega.set_target(target_omega);
			pid_omega.set_ref(data.omega);
			pid_omega.calc();
			out = pid_omega.get_out();

		} break;
		default: {
			out = 0.0f;
		} break;
	}
	output();

}

void M3508::init(CAN_HandleTypeDef *_hcan, dji_id_e _id, dji_control_e _method)
{
	if (_hcan->Instance == CAN1) {
		can_manager = &can1_manager;
	} else if (_hcan->Instance == CAN2) {
		can_manager = &can2_manager;
	}
	id = _id;
	method = _method;
	can_tx_data = allocate_tx_data(_hcan, _id);
}

void M3508::data_pross()
{
	static motor_measure_t tmp_measure;
	int16_t delta_encoder;
	// 数据处理过程
	get_motor_measure(&tmp_measure, can_manager->rx_buffer.data);

	// 计算圈数与总编码器值
	delta_encoder = tmp_measure.ecd - data.pre_encoder;
	if (delta_encoder < -encoder_num_pre_round / 2) {
		// 正方向转过了一圈
		data.total_round++;
	} else if (delta_encoder > encoder_num_pre_round / 2) {
		// 反方向转过了一圈
		data.total_round--;
	}
	data.total_encoder = data.total_round * encoder_num_pre_round +
	                     tmp_measure.ecd;

	data.angle =
	    (float)data.total_encoder / (float)encoder_num_pre_round * 2.0f * PI;
	data.omega = (float)tmp_measure.rpm * RPM_TO_OMEGA;
	data.torque = tmp_measure.current;
	data.temperature = tmp_measure.temperature + CELSIUS_TO_KELVIN;

	// 存储预备信息
	data.pre_encoder = tmp_measure.ecd;
}

void M3508::CAN_RxCpltCallback(uint8_t *rx_data) { data_pross(); }

void M3508::output()
{
	can_tx_data[0] = (int16_t)out >> 8;
	can_tx_data[1] = (int16_t)out;
}

void M3508::pid_calc()
{
	switch (method) {
		case (OPENLOOP_CTRL): {
			out = target_torque;
		} break;
		case (TORQUE_CTRL): {
			pid_torque.set_target(target_torque);
			pid_torque.set_ref(data.torque);
			pid_torque.calc();
			out = pid_torque.get_out();
		} break;
		case (OMEGA_CTRL): {
			pid_omega.set_target(target_omega);
			pid_omega.set_ref(data.omega);
			pid_omega.calc();
			out = pid_omega.get_out();
		} break;
		case (ANGLE_CTRL): {
			pid_angle.set_target(target_angle);
			pid_angle.set_ref(data.angle);
			pid_angle.calc();
			target_omega = pid_angle.get_out();

			pid_omega.set_target(target_omega);
			pid_omega.set_ref(data.omega);
			pid_omega.calc();
			out = pid_omega.get_out();

		} break;
		default: {
			out = 0.0f;
		} break;
	}
	output();
}

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
